import numpy as np
import time
class PoseSmoothing:
    def __init__(self, window_size=5, smoothing_factor=0.8):
        self.window_size = window_size
        self.smoothing_factor = smoothing_factor
        self.pose_history = []
        
    def smooth_pose(self, current_pose):
        """平滑姿态参数"""
        if current_pose is None:
            return None
            
        # 保存当前姿态
        self.pose_history.append(current_pose)
        if len(self.pose_history) > self.window_size:
            self.pose_history.pop(0)
            
        # 如果历史不足，返回当前姿态
        if len(self.pose_history) < 2:
            return current_pose
            
        # 指数平滑
        smoothed_pose = current_pose.copy()
        prev_pose = self.pose_history[-2]
        
        # 平滑SMPL参数
        if hasattr(smoothed_pose, 'body_pose') and hasattr(prev_pose, 'body_pose'):
            smoothed_pose.body_pose = self.smoothing_factor * smoothed_pose.body_pose + \
                                      (1 - self.smoothing_factor) * prev_pose.body_pose
                                      
        if hasattr(smoothed_pose, 'global_orient') and hasattr(prev_pose, 'global_orient'):
            smoothed_pose.global_orient = self.smoothing_factor * smoothed_pose.global_orient + \
                                         (1 - self.smoothing_factor) * prev_pose.global_orient
        
        return smoothed_pose